#include "klib.h"
#include <stdarg.h>

#if !defined(__ISA_NATIVE__) || defined(__NATIVE_USE_KLIB__)


void put_s(char* s, int len, int zerofill) {
  if (len==-1) {
    while (*s!='\0') {
    _putc(*s);
    s++;
    }
    return;
  }

  int s_len = strlen(s);
  if (zerofill) {
    if (len>s_len) {
      while(len-s_len) {
        _putc('0');
        len--;
      }
    }
  }
  while (*s!='\0'&&len) {
    _putc(*s);
    s++;
    len--;
  }
  return;
}

void put_i(int i, int base, int len, int zerofill) {
  if (i < 0) {
    _putc('-');
    i = -i;
  }

  if (i == 0) {
    if (zerofill&&len!=-1) {
      while(len-1) {
        _putc('0');
        len--;
      }
    }
    _putc('0');
    return;
  }
  
  int radix = 1;
  len--;
  while (i/(radix*base)) {radix*=base; len--;}
  if (zerofill&&len>0) {
    while(len>0) {
      _putc('0');
      len--;
    }
  }
  while (radix) {
    int q = i/radix;
    _putc((q>9)?'A'+q-10:'0'+q);
    i -= q*radix;
    radix/=base;
  }
  return;
}

int printf(const char *fmt, ...) {
  assert(fmt);
  int len = -1;
  int zerofill = 0;
  va_list ap;
  va_start(ap, fmt);
  while(*fmt!='\0') {
    if(*fmt!='%') {
      _putc(*fmt);
      fmt++;
    } else {
      fmt++;
      int condition=1;
      while (condition) {
        switch (*fmt) {
          case 's': put_s(va_arg(ap, char*), len, zerofill); condition=0; zerofill = 0; len = -1;break;
          case 'd': put_i(va_arg(ap, int), 10, len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case 'x': put_i(va_arg(ap, int), 16,  len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case 'p': put_i(va_arg(ap, int), 16, len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case '0': zerofill = 1; break;
          case '1':case '2':case '3':case '4': case '5':case '6': case '7': case '8': case '9': len = *fmt-'0';break;
          default: _putc(*fmt); assert(0);
        }
      fmt++;
      }
    }
  }
  va_end(ap);
  return 0;
}



int vsprintf(char *out, const char *fmt, va_list ap) {
  return 0;
}



int write_s(char** out, char* s, int len, int zerofill) {
  int ret = 0;
  assert(*out && s);
  if (len==-1) {
    while (*s!='\0') {
    **out = *s;
    s++;
    (*out)++;
    ret++;
    }
    return ret;
  }

  int s_len = strlen(s);
  if (zerofill) {
    if (len>s_len) {
      while(len-s_len) {
        **out = '0';
        ret++;
        len--;
        (*out)++;
      }
    }
  }
  while (*s!='\0'&&len) {
    **out = *s;
    ret++;
    s++;
    (*out)++;
    len--;
  }
  return ret;
}

int write_i(char** out, int i, int base, int len, int zerofill) {
  int ret = 0;
  assert(*out);
  if (i < 0) {
    **out = '-';
    (*out)++;
    ret++;
    i = -i;
  }

  if (i == 0) {
    if (zerofill&&len!=-1) {
      while(len-1) {
        **out='0';
        ret++;
        (*out)++;
        len--;
      }
    }
    **out = '0';
    ret++;
    (*out)++;
    return ret;
  }
  
  int radix = 1;
  len--;
  while (i/(radix*base)) {radix*=base; len--;}
  if (zerofill&&len>0) {
    while(len>0) {
      **out = '0';
      ret++;
      (*out)++;
      len--;
    }
  }
  while (radix) {
    int q = i/radix;
    **out = (q>9)?'A'+q-10 : '0'+ q;
    ret++;
    (*out)++;
    i -= q*radix;
    radix/=base;
  }
  return ret;
}

int sprintf(char *out, const char *fmt, ...) {
  assert(out && fmt);
  int num=0;
  int len = -1;
  int zerofill = 0;
  va_list ap;
  va_start(ap, fmt);
  while (*fmt!='\0') {
    if (*fmt != '%') {
      *out = *fmt;
      out++;
      fmt++;
      num++;
    } else {
      fmt++;
      int condition=1;
      while (condition) {
        switch (*fmt) {
          case 's': num+=write_s(&out, va_arg(ap, char*), len, zerofill); condition=0; zerofill = 0; len = -1;break;
          case 'd': num+=write_i(&out, va_arg(ap, int), 10, len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case 'x': num+=write_i(&out, va_arg(ap, int), 16, len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case 'p': num+=write_i(&out, va_arg(ap, int), 16, len, zerofill); condition=0; zerofill = 0; len = -1; break;
          case '0': zerofill = 1; break;
          case '1':case '2':case '3':case '4': case '5':case '6': case '7': case '8': case '9': len = *fmt-'0';break;
          default: assert(0);
        }
        fmt++;
      }
    }
  }
  assert(*fmt=='\0');
  *out = '\0';
  va_end(ap);
  return num;
}



int snprintf(char *out, size_t n, const char *fmt, ...) {
  return 0;
}

#endif
